#include"TAutomatonCoordinator.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"
#include "Comms/SerialPortManager/SerialPortManager.hpp"

using namespace std;

//const int TAutomatonFrameCoordinator::Subordinates::SubIndexSelf = 255;

TAutomatonFrameCoordinator::TAutomatonFrameCoordinator(GeneralSlave* ref) {
	genSlaveInit(ref);

	setDefaultFriendlyName();
}

TAutomatonFrameCoordinator::~TAutomatonFrameCoordinator() {
	genSlaveRelease();
}

struct BridgeCmd {
	struct Config {
		static U8Vec targetCmd(int hopNum, int target);
		static std::string targetReply(int target);

		static U8Vec checkIDCmd(int hopNum);
	};
};

// hop 0 to 101
// 0x12 0x00 0x31 0x30 0x31
// hop 1 to 1
// 0x12 0x01 0x30 0x30 0x31
U8Vec BridgeCmd::Config::targetCmd(int hopNum, int target) {
	U8Vec cmd;
	// bridge command
	cmd.push_back(0x12);
	// number of hops to reach this coordinator in bin format
	cmd.push_back(hopNum);

	string idxStr = TFormater::int2str(target);
	TFormater::str2u8(idxStr, cmd);
	cmd.push_back('\n');
	return cmd;
}

std::string BridgeCmd::Config::targetReply(int target) {
	const size_t bufferSize = 256;
	char ret[bufferSize];
	sprintf_s(ret, bufferSize, "Set bridge mode destine #%03d\r\n", target);
	return ret;
}

U8Vec BridgeCmd::Config::checkIDCmd(int hopNum) {
	U8Vec cmd;
	// control command
	cmd.push_back(0x11);
	// number of hops to reach this coordinator in bin format
	cmd.push_back(hopNum);
	// command code
	cmd.push_back(0x02);
	cmd.push_back('\n');
	return cmd;
}

bool TAutomatonFrameCoordinator::needConfigReroute(int callerIdx, int tempConfigSelf) {
	if (_comm.coordinator != nullptr) {
		auto* hop = (TAutomatonFrameCoordinator*)_comm.coordinator;
		// any root-wise coordinator needs config, this too
		if (hop->needConfigReroute(_comm.coordinatorIndex, -1)) {
			return true;
		}
	}
	// unless just temp configure this-self
	if (tempConfigSelf >= 0) {
		return false;
	}
	// or match current config
	return callerIdx != _subs.selectedSubIndex;
}

// bookmark: slave: bridge name
bool TAutomatonFrameCoordinator::updateBridgedNameInLog(std::wstring baseCallerName, int callerIdx, int callerIsAE) {
	if (_comm.coordinator != nullptr) {
		auto* hop = (TAutomatonFrameCoordinator*)_comm.coordinator;
		return hop->updateBridgedNameInLog(baseCallerName, callerIdx, -1);
	}
	else {
		formatNameInLogAndCallerIdx(baseCallerName, callerIdx, callerIsAE);
		return true;
	}
}

// see also: GeneralSlave::preSendingConfig()
int TAutomatonFrameCoordinator::preSendingConfig(std::wstring baseCallerName, int callerIdx, int tempConfigSelf, int callerIsAE) {
	if (callerIdx > 0 && callerIdx < 255) {
		if (!_subs.debug_allSubIndexes.contains(callerIdx)) {
			_subs.debug_allSubIndexes.insert(callerIdx);
		}
	}

	// tempConfigSelf handled here
	if (!needConfigReroute(callerIdx, tempConfigSelf)) {
		// no need to configure. just update the name
		updateBridgedNameInLog(baseCallerName, callerIdx, callerIsAE);
		return 1;
	}

	// config reroute to caller
	string blockTxt;
	if (isLoggingDetails()) {
		blockTxt = "preSendingConfig on " + TFormater::wstring2string(getReferenceName());
	}

	TLogger block(blockTxt);
	if (_comm.coordinator != nullptr) {
		auto* hop = (TAutomatonFrameCoordinator*)_comm.coordinator;
		// config coordinators further root-wise
		if (isLoggingDetails()) {
			TLogger::writeLine("Config parent coord");
		}
		int ret = hop->preSendingConfig(baseCallerName, _comm.coordinatorIndex);
		if (ret <= 0) {
			return ret;
		}
	}

	// all root-wise coords configed
	// configure this one now
	callerIdx = formatNameInLogAndCallerIdx(baseCallerName, callerIdx, callerIsAE);

	//if (_callbacks.nameInCB.compare(L"daisy_chain_root/robot_platform") == 0) {
	//	int stop = 1;
	//}

	if (isLoggingDetails()) {
		TLogger::writeLine("Check connect");
	}

	if (isConnected() <= 0) {
		return 0;
	}

	// config this 
	if (callerIdx == _subs.selectedSubIndex) {
		return 1;
	}
	else {
		if (isLoggingDetails()) {
			TLogger::writeLine("Check connection");
		}
		//if (confirmConnect() <= 0) {
		//	return 0;
		//}

		// configure coordinator routing
		auto cmd = BridgeCmd::Config::targetCmd(getHopNum(), callerIdx);
		auto exp = BridgeCmd::Config::targetReply(callerIdx);

		RoutingCallNameSwitcher nameSW(&_callbacks.nameInCB, &_subs.nameInRoutingCB);

		if (sendU8(cmd) > 0) {
			int ret = checkReply(exp);
			if (ret > 0) {
				_subs.selectedSubIndex = callerIdx;
				nameSW.routineComplete();
			}
			else {
				_comm.commHandle->purge();
			}
			return ret;
		}
	}

	return 0;
}

int TAutomatonFrameCoordinator::detectSubordinate(int index, std::wstring& who, std::wstring& which) {
	if (select(index)) {
		HandShakeProtocol handshake = GeneralSlave::makeGeneralSerialHandshake(vector<string>(), vector<string>(), L"", L"", 1);
		for (auto& shake : handshake.handshakes) {
			if (shake.toSend.dataToSend.size() > 0) {
				if (_comm.commHandle->sendBytes(shake.toSend.dataToSend) > 0) {
					auto reply = _comm.commHandle->receiveStrReplyWithDelim(shake.exp.timeout, '\n');
					reply = TFormater::removeEoL(reply);
					auto replyW = TFormater::string2wstring(reply);
					if (TFormater::beginWith(shake.toSend.dataToSend, "who")) {
						who = replyW;
					}
					else if (TFormater::beginWith(shake.toSend.dataToSend, "which")) {
						which = replyW;
					}
				}
				else {
					//shake.result.
					return 0;
				}
			}
			else {
				_comm.commHandle->receiveStrReplyWithDelim(shake.exp.timeout, '\n');
			}
		}

		return 1;
	}
	return 0;
}

int TAutomatonFrameCoordinator::select(int index) {
	string cmd = "select " + TFormater::int2str(index) + "\r\n";
	string exp = "on " + TFormater::int2str(index) + "\r\n";
	if (_comm.commHandle->sendString(cmd) > 0) {
		string reply = _comm.commHandle->receiveStrReplyWithDelim(_comm.defaultTimeout, '\n');
		return reply.compare(exp) == 0;
	}
	return 0;
}

int TAutomatonFrameCoordinator::getHopNum() {
	int hopNum = 0;
	GeneralSlave* hop = _comm.coordinator;
	while (hop != nullptr) {
		hop = hop->_comm.coordinator;
		hopNum++;
	}
	return hopNum;
}

int TAutomatonFrameCoordinator::connectThroughTBridge(int discoverMode) {
	int ret = 0;
	if (_comm.connectionType.compare(L"USB") == 0) {
		// like 
		// GeneralSlave::connectThroughUSB 
		// but with one ex handshake first
		HandShakeProtocol handshakes = makeBridgeRootSerialHandshake(_meta.model, _comm.identifier, getHopNum(), discoverMode);
		vector<wstring> hwids;
		if (_comm.appearance.length() > 0) {
			hwids.push_back(_comm.appearance);
		}
		else {
			hwids = HardwareList::getSerialHWIDs();
		}

		ret = connectThroughUSBExe(handshakes, hwids, discoverMode);
	}
	else {
		ret = connectThroughTBridgeNonRoot(discoverMode);
	}
	if (ret > 0) {
		updateAfterConnectAny(nullptr);
	}
	return ret;
}

int TAutomatonFrameCoordinator::connectThroughTBridgeNonRoot(int discoverMode) {
	// difference from generalslave: add stopHop
	// generalslave::connectThroughTBridgeNonRoot
	HandShakeProtocol handshakes =
		makeBridgeRootSerialHandshake(_meta.model, _comm.identifier, getHopNum(), discoverMode);
	// search for root comm channel
	GeneralSlave* rootCrdr = _comm.coordinator;
	while (rootCrdr->_comm.coordinator != nullptr) {
		rootCrdr = rootCrdr->_comm.coordinator;
	}
	auto* rootChannel = rootCrdr->_comm.commHandle;
	// see
	// CommChannelSerial::executeHandshake
	if (rootChannel->executeHandshake(handshakes, _comm.SOFT_TIMEOUT_TBRIDGE) > 0) {
		updateBeforeConnectThroughBridge(rootCrdr);
		//_comm._conStat = ConnectionStatus::Connected;
		return 1;
	}
	//}

	return 0;
}

//void TAutomatonFrameCoordinator::updateAfterConnectAny(HandShakeProtocol* protocol) {
//	GeneralSlave::updateAfterConnectAny(protocol);
//	if (_comm.embodiment != nullptr) {
//		shareLink(_comm.embodiment);
//	}
//}

int TAutomatonFrameCoordinator::confirmConnected() {
	if (_comm._conStat != ConnectionStatus::Connected) {
		return -1;
	}
	if (preSendingConfig(getReferenceName(), _comm.coordinatorIndex, 1) < 0) {
		return -1;
	}
	return confirmConnectCore(1);
}

int TAutomatonFrameCoordinator::reConnect() {
	if (preSendingConfig(getReferenceName(), _comm.coordinatorIndex, 1) < 0) {
		return -1;
	}
	if (confirmConnectCore(0) <= 0) {
		if (_comm.coordinator != nullptr) {
			if (_comm.coordinator->reConnect() < 0) {
				// wait for coordinator to recover
				return 0;
			}
			if (confirmConnectCore(0) <= 0) {
				return -1;
			}
		}
		// reconnect this, if controlling interface 
		if (_comm.isCommChOwner > 0) {
			if (_comm.commHandle->reconnect() > 0) {
				return confirmConnectCore(0);
			}
			return -1;
		}
	}
	return 1;
}

int TAutomatonFrameCoordinator::forceRebuildBridges() {
	_subs.selectedSubIndex = -100;
	if (_comm.coordinator != nullptr) {
		return _comm.coordinator->forceRebuildBridges();
	}
	return 1;
}

HandShakeProtocol TAutomatonFrameCoordinator::makeBridgeRootSerialHandshake(std::wstring model, std::wstring id, int hopNum, int discoveryMode) {
	// like 
	// GeneralSlave::makeGeneralSerialHandshake 
	// but with a init
	HandShakeProtocol ret;
	{
		HandShakeProtocol::OneShake configNoHop("stopHop");

		auto cmd = BridgeCmd::Config::targetCmd(hopNum, Subordinates::SubIndexSelf);
		auto exp = BridgeCmd::Config::targetReply(Subordinates::SubIndexSelf);
		configNoHop.set(cmd, exp, globalSlaveInitializer._comm.defaultTimeout);
		ret.handshakes.push_back(configNoHop);
	}
	makeGeneralSerialHandshakeCore(ret, vector<string>(), vector<string>(), model, id, discoveryMode);

	return ret;
}

int TAutomatonFrameCoordinator::confirmConnectCore(int logWarningOnFailure) {
	if (_comm._conStat == ConnectionStatus::Missing) {
		return 0;
	}
	purgeReply();
	bool idMatch = false;
	auto cmd = BridgeCmd::Config::checkIDCmd(getHopNum());
	auto explyID = TFormater::wstring2string(getID()) + "\r\n";
	for (int loop = 0; loop < 3; loop++) {
		if (sendU8(cmd) > 0) {
			int ret = checkReply(explyID);
			if (ret > 0) {
				idMatch = true;
				break;
			}
			else {
				Sleep(100);
				purgeReply();
			}
		}
	}
	if (!idMatch) {
		if (logWarningOnFailure > 0) {
			TLogger::writeWarnning(L"Broken link detected for " + getReferenceName());
		}
		if (_comm._conStat != ConnectionStatus::Broken) {
			_comm.markBroken();
		}
		return 0;
	}
	else {
		if (_comm._conStat != ConnectionStatus::Connected) {
			_comm.markRecovery();
		}
		return 1;
	}
}

int TAutomatonFrameCoordinator::formatNameInLogAndCallerIdx(std::wstring baseCallerName, int callerIdx, int callerIsAE) {
	if (callerIsAE > 0) {
		// caller is this-self but in the name of an avatar/embodiment
		callerIdx = _subs.SubIndexSelf;
		_callbacks.nameInCB = baseCallerName;
		_subs.nameInRoutingCB = baseCallerName + L"(" + _meta.friendlyName + L")";
	}
	else {
		if (callerIdx < 0) {
			// caller is this-self
			callerIdx = _subs.SubIndexSelf;
			_callbacks.nameInCB = _meta.friendlyName;
			_subs.nameInRoutingCB = _meta.friendlyName;
		}
		else {
			_callbacks.nameInCB = _meta.friendlyName + L"/" + baseCallerName;
			_subs.nameInRoutingCB = _callbacks.nameInCB + L"/..";
		}
	}

	if (_comm.embodiment != nullptr) {
		// actually send through the embodiment
		_comm.embodiment->_callbacks.nameInCB = _callbacks.nameInCB;
	}
	return callerIdx;
}

TAutomatonFrameCoordinator::RoutingCallNameSwitcher::RoutingCallNameSwitcher(std::wstring* nameInCBRefIn, std::wstring* nameInRoutingRefIn) {
	nameInCBRef = nameInCBRefIn;
	nameInRoutingRef = nameInRoutingRefIn;

	nameInCB_backup = *nameInCBRef;
	*nameInCBRef = *nameInRoutingRef;
}

void TAutomatonFrameCoordinator::RoutingCallNameSwitcher::routineComplete() {

}

TAutomatonFrameCoordinator::RoutingCallNameSwitcher::~RoutingCallNameSwitcher() {
	*nameInCBRef = nameInCB_backup;
}